Program Listing for File estimator.h

↰ Return to documentation for file (codes/slam/estimator/estimator.h)

/*******************************************************
 * Copyright (C) 2019, Robotics Group, Nanyang Technology University
 *
 * \file estimator.h
 * \author Zhang Handuo (hzhang032@e.ntu.edu.sg)
 * \date Januarary 2017
 * \brief SLAM main process of SSLAM-pose_graph.
 *
 * Licensed under the GNU General Public License v3.0;
 * you may not use this file except in compliance with the License.
 *
 *******************************************************/

#pragma once

#include <thread>
#include <mutex>
#include <atomic>
#include <std_msgs/Header.h>
#include <std_msgs/Float32.h>
#include <ceres/ceres.h>
#include <unordered_map>
#include <queue>
#include <opencv2/core/eigen.hpp>
#include <eigen3/Eigen/Dense>
#include <eigen3/Eigen/Geometry>

#include "parameters.h"
#include "feature_manager.h"
#include "../utility/utility.h"
#include "../utility/tic_toc.h"
#include "../initial/solve_5pts.h"
#include "../initial/initial_sfm.h"
#include "../initial/initial_alignment.h"
#include "../initial/initial_ex_rotation.h"
#include "../factor/imu_factor.h"
#include "../factor/ins_factor.h"
#include "../factor/pose_local_parameterization.h"
#include "../factor/marginalization_factor.h"
#include "../factor/projectionTwoFrameOneCamFactor.h"
#include "../factor/projectionTwoFrameTwoCamFactor.h"
#include "../factor/projectionOneFrameTwoCamFactor.h"
#include "../featureTracker/feature_tracker.h"

using namespace noiseFactor;

namespace slam_estimator {
    class Estimator {
    public:
    #ifndef DOXYGEN_SHOULD_SKIP_THIS
            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
    #endif /* DOXYGEN_SHOULD_SKIP_THIS */

        Estimator();

        ~Estimator();

        void setParameter();

        void startProcessThread();

        // interface
        void initFirstPose(const Eigen::Vector3d &p, const Eigen::Matrix3d r);


        void inputIMU(double t, const Vector3d &linearAcceleration, const Vector3d &angularVelocity);

        void inputINS(double t, const Vector3d &linearSpeed, const Quaterniond &angularRead, const double height);

        void inputFeature(double t, const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &featureFrame);

        void
        inputImage(double t, const cv::Mat &_img, const cv::Mat &_img1 = cv::Mat(), const cv::Mat &_mask = cv::Mat());

        void processIMU(double t, double dt, const Vector3d &linear_acceleration, const Vector3d &angular_velocity);

        void processINS(double t, double dt, const Vector3d &linear_speed,
                        const Quaterniond &angular_read, const double height_, const bool last_);

        void processImage(const map<int, vector<pair<int, Eigen::Matrix<double, 7, 1>>>> &image, const double header);

        void processMeasurements();

        void clearState();

        bool initialStructure();

        bool visualInitialAlign();

        bool relativePose(Matrix3d &relative_R, Vector3d &relative_T, int &l);

        void slideWindow();

        void slideWindowNew();

        void slideWindowOld();

        //***************************************************************************************
        //
        //
        //***************************************************************************************
        void optimization();

        //***************************************************************************************
        //
        //***************************************************************************************
        void vector2double();

        //***************************************************************************************
        //
        //***************************************************************************************
        void double2vector();

        bool failureDetection();

        bool getIMUInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &accVector,
                            vector<pair<double, Eigen::Vector3d>> &gyrVector);

        bool getINSInterval(double t0, double t1, vector<pair<double, Eigen::Vector3d>> &spdVector,
                            vector<pair<double, Eigen::Quaterniond>> &angVector,
                            vector<pair<double, double>> &heightVector);

        void getPoseInWorldFrame(Eigen::Matrix4d &T);

        void getPoseInWorldFrame(int index, Eigen::Matrix4d &T);

        void predictPtsInNextFrame();

        void outliersRejection(set<int> &removeIndex);

        double reprojectionError(Matrix3d &Ri, Vector3d &Pi, Matrix3d &rici, Vector3d &tici,
                                 Matrix3d &Rj, Vector3d &Pj, Matrix3d &ricj, Vector3d &ticj,
                                 double depth, Vector3d &uvi, Vector3d &uvj);

        void updateLatestStates();

        void fastPredictIMU(double t, const Eigen::Vector3d &linear_acceleration,
                            const Eigen::Vector3d &angular_velocity);

        void fastPredictINS(double t, const Eigen::Vector3d &linear_speed,
                            const Eigen::Quaterniond &angular_read);

        bool IMUAvailable(double t);

        bool INSAvailable(double t);

        void initFirstIMUPose(vector<pair<double, Eigen::Vector3d>> &accVector);

        void initFirstINSPose(vector<pair<double, Eigen::Vector3d>> &spdVector,
                              vector<pair<double, Eigen::Quaterniond>> &angVector,
                              vector<pair<double, double>> heightVector);

        enum SolverFlag {
            INITIAL,
            NON_LINEAR
        };

        enum MarginalizationFlag {
            MARGIN_OLD = 0,
            MARGIN_SECOND_NEW = 1
        };

        unsigned int count_;
        std::mutex mBuf;
        std::mutex mProcess;
        queue<pair<double, Eigen::Vector3d>> accBuf;
        queue<pair<double, Eigen::Vector3d>> gyrBuf;
        queue<pair<double, Eigen::Vector3d>> spdBuf;
        queue<pair<double, Eigen::Quaterniond>> angBuf;
        queue<pair<double, double>> heightBuf;
        queue<pair<double, map<int, vector<pair<int, Eigen::Matrix<double, 7, 1> > > > > > featureBuf;
        vector<pair<double, vector<double>>> gpsVec;
        double prevTime, curTime;
        bool openExEstimation;

//    std::thread trackThread;
        std::thread processThread;
        atomic<bool> processThread_swt;  // this goes in while(1) aka inf-while of processThread

        FeatureTracker featureTracker;

        SolverFlag solver_flag;
        MarginalizationFlag marginalization_flag;
        Vector3d g;

        Matrix3d ric[2];
        Vector3d tic[2];

        Vector3d Ps[(WINDOW_SIZE + 1)];
        Vector3d Vs[(WINDOW_SIZE + 1)];
        Matrix3d Rs[(WINDOW_SIZE + 1)];
        Vector3d Bas[(WINDOW_SIZE + 1)];
        Vector3d Bgs[(WINDOW_SIZE + 1)];
        double td;

        Matrix3d back_R0, last_R, last_R0;
        Vector3d back_P0, last_P, last_P0;
        double Headers[(WINDOW_SIZE + 1)];
        double last_time;

        IntegrationBase *pre_integrations[(WINDOW_SIZE + 1)];
        Vector3d acc_0, gyr_0;
        Quaterniond ang_0;

        vector<double> dt_buf[(WINDOW_SIZE + 1)];
        vector<double> t_buf[(WINDOW_SIZE + 1)];
        vector<Vector3d> linear_acceleration_buf[(WINDOW_SIZE + 1)];
        vector<Vector3d> angular_velocity_buf[(WINDOW_SIZE + 1)];
        vector<Vector3d> linear_speed_buf[(WINDOW_SIZE + 1)];
        vector<Quaterniond> angular_read_buf[(WINDOW_SIZE + 1)];
//    vector<double> height_read_buf[(WINDOW_SIZE + 1)];
        double sum_dt[(WINDOW_SIZE + 1)];

        int frame_count;
        int sum_of_outlier, sum_of_back, sum_of_front, sum_of_invalid;
        int inputImageCnt;

        FeatureManager f_manager;
        MotionEstimator m_estimator;
        InitialEXRotation initial_ex_rotation;

        bool first_imu, first_ins;
        bool is_valid, is_key;
        bool failure_occur;

        vector<Vector3d> point_cloud;
        vector<Vector3d> margin_cloud;
        vector<Vector3d> key_poses;
        double initial_timestamp;

        double para_Pose[WINDOW_SIZE + 1][SIZE_POSE];
        double para_SpeedBias[WINDOW_SIZE + 1][SIZE_SPEEDBIAS];
        double para_Feature[NUM_OF_F][SIZE_FEATURE];
        double para_Ex_Pose[2][SIZE_POSE];
        double para_Retrive_Pose[SIZE_POSE];
        double para_Td[1][1];
        double para_Tr[1][1];

        double sensor_h;

        int loop_window_index;

        MarginalizationInfo *last_marginalization_info;
        vector<double *> last_marginalization_parameter_blocks;

        map<double, ImageFrame> all_image_frame;
        IntegrationBase *tmp_pre_integration;

        Eigen::Matrix3d cov_position;
        Eigen::Vector3d initP;
        Eigen::Matrix3d initR;

        double latest_time;
        Eigen::Vector3d latest_P, latest_V, latest_Ba, latest_Bg,
                latest_acc_0, latest_gyr_0, last_vec_rev, latest_spd_0;
        Eigen::Quaterniond latest_Q, last_ang_rev;

        bool initFirstPoseFlag;
        bool initThreadFlag;
    };
}